#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
#include <rt_link.h>
#include <nrk_error.h>
//#include <sys/time.h>

#define MY_CHANNEL 17

#define MAX_MOLES  5

#define MY_TX_SLOT  0
#define MOLE_1_RX   2 
#define MOLE_2_RX   4 
#define MOLE_3_RX   6 
#define MOLE_4_RX   8 
#define MOLE_5_RX   10 



NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);


void nrk_create_taskset();

uint8_t tx_buf[RF_MAX_PAYLOAD_SIZE];
uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE];

nrk_time_t timestart;
nrk_time_t timeend;
nrk_time_t newtime;
nrk_time_t timeout;



int
main ()
{
  uint16_t div;
  nrk_setup_ports();
  nrk_setup_uart(UART_BAUDRATE_115K2);

  nrk_kprintf( PSTR("Starting up...\r\n") );
	
  nrk_init();

  nrk_led_clr(0);
  nrk_led_clr(1);
  nrk_led_clr(2);
  nrk_led_clr(3);
  
  nrk_time_set(0,0);

  
  rtl_task_config();
  
  nrk_create_taskset ();

  nrk_start();
  
  return 0;
}


void Task1()
{

  uint8_t j, i,last_mole,m;
  uint8_t cnt,cmd,length;
  uint8_t rssi,slot,oldMole,newMole=0,Rounds =0 ;
  uint8_t number_timeouts =0;
  uint8_t *local_rx_buf;
  uint16_t counter;
  uint32_t Score = 0;
  volatile nrk_time_t t;
  char c;
  nrk_sig_t uart_rx_signal;

  printf( "Task1 PID=%d\r\n",nrk_get_pid());
  counter=0;
  cnt=0;
  
  nrk_led_set(RED_LED);
  
  rtl_init (RTL_COORDINATOR);
  
  rtl_set_schedule( RTL_TX, MY_TX_SLOT, 1 );
  rtl_set_schedule( RTL_RX, MOLE_1_RX, 1 );
  rtl_set_schedule( RTL_RX, MOLE_2_RX, 1 );
  rtl_set_schedule( RTL_RX, MOLE_3_RX, 1 );
  rtl_set_schedule( RTL_RX, MOLE_4_RX, 1 );
  rtl_set_schedule( RTL_RX, MOLE_5_RX, 1 );

  rtl_set_channel(MY_CHANNEL);
  rtl_start();
  rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
  
  while(!rtl_ready())  nrk_wait_until_next_period(); 

  // Get the signal for UART RX
  uart_rx_signal=nrk_uart_rx_signal_get();
  // Register task to wait on signal
  nrk_signal_register(uart_rx_signal); 


  // This shows you how to wait until a key is pressed to start
  nrk_kprintf( PSTR("Press 's' to start\r\n" ));


do{
  if(nrk_uart_data_ready(NRK_DEFAULT_UART))
	c=getchar();
  else nrk_event_wait(SIG(uart_rx_signal));
  nrk_time_get(&timestart);
  nrk_time_get(&timeout);
} while(c!='s');

    while(Rounds<=10)
         {

	 if( rtl_tx_pkt_check(MY_TX_SLOT)!=0 )
               {
                  //printf( "Pending on slot %d\r\n",MY_TX_SLOT );
               }
          else {

	        nrk_time_get(&timeend);
		if (Rounds<=9){
		  if(timeend.secs-timeout.secs > (8-Rounds/2))
		  {
		    oldMole= newMole;
		    while(oldMole==newMole)
		      newMole = rand()%MAX_MOLES;
		    nrk_time_get(&timeout);
		    Rounds++;
		    number_timeouts++;
		    printf("\n\n\n\n\n\nRounds = %d \nnumber_timeouts = %d \npresent time = %d\n", Rounds, number_timeouts, timeout.secs-timestart.secs);
		  }
		}
		 // added the next mole to light up into the buffer
		sprintf( &tx_buf[PKT_DATA_START],"Master count is %d and new mole is %d and Round = %d",counter,newMole,Rounds);
		// PKT_DATA_START + length of string + 1 for null at end of string
		if(Rounds>=10){
		  
		  Rounds++;
		sprintf( &tx_buf[PKT_DATA_START],"Master count is %d and new mole is %d and Round = %d",counter,6,Rounds); 
		}

		length=strlen(&tx_buf[PKT_DATA_START])+PKT_DATA_START+1;
		rtl_tx_pkt( tx_buf, length, MY_TX_SLOT);
                printf( "\nTX on slot %d\r\n",MY_TX_SLOT);
                for(i=PKT_DATA_START;i<length;i++)
                   printf("%c",local_rx_buf[i]);
		nrk_led_toggle(BLUE_LED);
		printf("\n\n");

                
	
	}
 

	  // Check for received packet	
	  if( rtl_rx_pkt_check()!=0 )
	       {
	       	   uint8_t mole_index,state;
		   local_rx_buf=rtl_rx_pkt_get(&length, &rssi, &slot);
		   printf( "RX slot %d %d: ",slot,length );
		   // print the packet out
		   for(i=PKT_DATA_START; i<length; i++ )
			printf( "%c",local_rx_buf[i] );
 		  
                   //buffer position 8 stores the value of the moleid from the slaves
                   //buffer position 16 stores the value of light 
                   // '1' indicates mole whacked (light closed)
                   // '0' indicates mole not whacked yet (light open)
                   if( ((local_rx_buf[8]-48) == newMole) && (local_rx_buf[16]=='1') && Rounds <=9)
                   {
		     printf("NEWMOLE:%d",newMole);
                     oldMole = newMole;
                     while(oldMole==newMole)
		     newMole = rand()%MAX_MOLES;
                     
                     Rounds++;
		     
                     printf("\n rounds %d",Rounds);
                    nrk_time_get(&timeend);
		    nrk_time_get(&timeout);
                    Score = timeend.secs-timestart.secs + number_timeouts * 10;
		    
                    printf("\nScore : %d",Score);

                   }
		   printf( "\r\n" ); 
		   rtl_rx_pkt_release();
	       } 
	  
	  rtl_wait_until_rx_or_tx();
	  
  	}
        nrk_time_get(&timeend);
        Score = timeend.secs-timestart.secs + number_timeouts*10;
        printf("DONE and Score = %d",Score);
        while(1)
        {
         rtl_wait_until_rx_or_tx(); 
        }
}


void
nrk_create_taskset()
{


  TaskOne.task = Task1;
  TaskOne.Ptos = (void *) &Stack1[NRK_APP_STACKSIZE-1];
  TaskOne.Pbos = (void *) &Stack1[0];
  TaskOne.prio = 2;
  TaskOne.FirstActivation = TRUE;
  TaskOne.Type = BASIC_TASK;
  TaskOne.SchType = PREEMPTIVE;
  TaskOne.period.secs = 1;
  TaskOne.period.nano_secs = 0;
  TaskOne.cpu_reserve.secs = 0;
  TaskOne.cpu_reserve.nano_secs = 100*NANOS_PER_MS;
  TaskOne.offset.secs = 0;
  TaskOne.offset.nano_secs= 0;
  nrk_activate_task (&TaskOne);


  nrk_kprintf( PSTR("Create Done\r\n") );
}


